import java.util.*;
import java.util.Map.*;

public class ShortestPath {

	static int n;
	public static ArrayList<Integer> arrayList;
	public static LinkedHashSet<String> setOfTurnRestrictions = new LinkedHashSet<String>();
	public static HashMap<String, Integer> hashMapOfMappingNumbers = new HashMap<String, Integer>();
	public static double linkcost[][];
	public static HashMap<Integer, Node> hashNode = new HashMap<Integer, Node>();
	private static int prevlink[][];
	private static double linkendcost[][];
	private static double turnpenalty[][];
	private static HashMap<Integer, Edge> hashEdge = new HashMap<Integer, Edge>();
	private static ArrayList<Integer> labelledlinks = new ArrayList<Integer>();
	private static ArrayList<Integer> unlabelledlinks = new ArrayList<Integer>();
	private static Queue<Integer> queue = new LinkedList<Integer>();
	public static Set<Integer> linkSet = new LinkedHashSet<Integer>();

	public static double haversineDistance(double lat1, double lng1,
			double lat2, double lng2) {
		double earthRadius = 3958.75;
		double dLat = Math.toRadians(lat2 - lat1);
		double dLng = Math.toRadians(lng2 - lng1);
		double a = Math.sin(dLat / 2) * Math.sin(dLat / 2)
				+ Math.cos(Math.toRadians(lat1))
				* Math.cos(Math.toRadians(lat2)) * Math.sin(dLng / 2)
				* Math.sin(dLng / 2);
		double c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1 - a));
		double dist = earthRadius * c;

		int meterConversion = 1609;

		return new Double(dist * meterConversion).doubleValue();
	}

	public static void createAdjmatrix() {

		 double inf = Double.POSITIVE_INFINITY;
//		 linkcost = new double[][] { { 0, 1, inf, inf, inf, inf, inf, inf },
//		 { inf, 0, 1, inf, 2, inf, inf, inf },
//		 { inf, inf, 0, 2, inf, 3, inf, inf },
//		 { inf, inf, inf, 0, inf, inf, 3, inf },
//		 { inf, inf, inf, inf, 0, 4, inf, inf },
//		 { inf, inf, inf, inf, inf, 0, 3, inf },
//		 { inf, inf, inf, inf, inf, inf, 0, 2 },
//		 { inf, inf, inf, inf, inf, inf, inf, 0 } };

		int counter = 1;

		for (int i = 0; i < n; i++) {
			for (int j = 0; j < n; j++) {
				if (hashNode.get(i) != null && hashNode.get(j) != null) {
					 if (linkcost[i][j] != Double.POSITIVE_INFINITY) {
					 linkcost[i][j] = haversineDistance(
					 hashNode.get(i).getLatitude(), hashNode.get(i)
					 .getLongitude(), hashNode.get(j)
					 .getLatitude(), hashNode.get(j)
					 .getLongitude());
					 linkcost[j][i] = Double.POSITIVE_INFINITY;

					Edge edge = new Edge(counter, i, j, linkcost[i][j]);
					hashEdge.put(counter, edge);
					unlabelledlinks.add(counter);
					 }

				} else {
					 linkcost[i][j] = Double.POSITIVE_INFINITY;
				}
				linkendcost[i][j] = Double.POSITIVE_INFINITY;
				counter++;
			}

		}

	}

	public static void turnRestrictedShortestPath(int origin) {
		int minI = 0, minJ = 0, minK = 0;
		int outgoingNode = 0;
		for (int i = 0; i < n; i++) {
			if (linkcost[origin][i] != Double.POSITIVE_INFINITY
					&& linkcost[origin][i] != 0.0) {

				Iterator entries = hashEdge.entrySet().iterator();
				while (entries.hasNext()) {
					Entry entry = (Entry) entries.next();
					Integer key = (Integer) entry.getKey();
					Edge value = (Edge) entry.getValue();
					if (value.getIncoming() == origin
							&& value.getOutgoing() == i) {
						queue.add(key.intValue());
					}
				}
			}
		}

		while (queue.size() != 0) {
			Integer poppedLink = queue.poll();
			outgoingNode = hashEdge.get(poppedLink).getOutgoing();
			unlabelledlinks.remove(poppedLink);
			labelledlinks.add(poppedLink);
			linkendcost[origin][outgoingNode] = linkcost[origin][outgoingNode];
			prevlink[origin][outgoingNode] = 0;
		}

		Integer unprocessedLink, unprocessedIncomingNode, unprocessedOutgoingNode, prevNode;
		prevNode = origin;
		minI = origin;
		minJ = origin;
		minK = outgoingNode;
		while (unlabelledlinks.size() != 0) {
			unprocessedLink = unlabelledlinks.remove(0);
			unprocessedIncomingNode = hashEdge.get(unprocessedLink)
					.getIncoming();
			unprocessedOutgoingNode = hashEdge.get(unprocessedLink)
					.getOutgoing();

			/**/
			double previousMinValue=Double.POSITIVE_INFINITY;
			ArrayList<Integer> linkNumbers = new ArrayList<Integer>();
			for (Integer i : hashEdge.keySet()) {
				if (linkendcost[hashEdge.get(i).getIncoming()][unprocessedIncomingNode] != 0
						&& linkendcost[hashEdge.get(i).getIncoming()][unprocessedIncomingNode] != Double.POSITIVE_INFINITY) {
					linkNumbers.add(hashEdge.get(i).getNumber());
					if(linkendcost[hashEdge.get(i).getIncoming()][unprocessedIncomingNode]<previousMinValue){
						previousMinValue=linkendcost[hashEdge.get(i).getIncoming()][unprocessedIncomingNode];
						prevNode=hashEdge.get(i).getIncoming();
					}
				}
			}
			

			if ((linkendcost[prevNode][unprocessedIncomingNode]
					+ turnPenalty(prevNode, unprocessedIncomingNode,
							unprocessedOutgoingNode) + linkcost[unprocessedIncomingNode][unprocessedOutgoingNode]) <= linkendcost[unprocessedIncomingNode][unprocessedOutgoingNode]) {
				if ((linkcost[unprocessedIncomingNode][unprocessedOutgoingNode] != Double.POSITIVE_INFINITY)
						&& (linkcost[unprocessedIncomingNode][unprocessedOutgoingNode] != 0.0)) {

					ArrayList<Integer> nodeArrayList = new ArrayList<Integer>();
					for (int i = 0; i < n; i++) {
						if ((linkcost[i][unprocessedIncomingNode] != Double.POSITIVE_INFINITY)
								&& (linkcost[i][unprocessedIncomingNode] != 0)) {
							nodeArrayList.add(i);
						}
					}
					double cost = Double.POSITIVE_INFINITY;
					linkendcost[unprocessedIncomingNode][unprocessedOutgoingNode] = cost;

					for (int i = 0; i < nodeArrayList.size(); i++) {
						double cost2 = linkcost[nodeArrayList.get(i)
								.intValue()][unprocessedIncomingNode]
								+ turnPenalty(nodeArrayList.get(i).intValue(),
										unprocessedIncomingNode,
										unprocessedOutgoingNode)
								+ linkcost[unprocessedIncomingNode][unprocessedOutgoingNode];

						if (cost2 < cost) {

							linkendcost[unprocessedIncomingNode][unprocessedOutgoingNode] = cost2;
							minI = nodeArrayList.get(i).intValue();
							minJ = unprocessedIncomingNode;
							minK = unprocessedOutgoingNode;
							Iterator entries = hashEdge.entrySet().iterator();
							while (entries.hasNext()) {
								Entry entry = (Entry) entries.next();
								Integer key = (Integer) entry.getKey();
								Edge value = (Edge) entry.getValue();
								if (value.getIncoming() == minI
										&& value.getOutgoing() == minJ) {
									prevlink[minJ][minK] = value.getNumber();
								}
							}
							cost = cost2;
						}
					}

				}
			}
			labelledlinks.add(unprocessedLink);
		}
		int minPreviousNode = 0;
		minK = ShortestPath.n - 1;
		double minPreviousDistance = Double.POSITIVE_INFINITY;

		for (int i = 0; i < n; i++) {
			if (linkcost[i][minK] < minPreviousDistance
					&& linkcost[i][minK] != 0) {
				minPreviousDistance = linkcost[i][minK];
				minPreviousNode = i;
			}
		}

		displayPath(minPreviousNode, minK, origin);
		display();
		displayLinkendCost();
	}

	public static double turnPenalty(int i, int j, int k) {
		if ((i == 1 && j == 4 && k == 5)||(i == 10 && j == 4 && k == 5)) {
			setOfTurnRestrictions.addAll(Arrays.asList("0,3,4"));
			return Double.POSITIVE_INFINITY;
		}

		else
			return 0;
	}

	private static int linkNo;

	public static void displayPath(int i, int j, int origin) {
		if (i == origin) {
			//linkSet.add(j);
			linkSet.add(origin);
			return;
		}

		if (i <= j) {
			linkSet.add(j);
			linkSet.add(i);
		} else {
			linkSet.add(i);
			linkSet.add(j);

		}
		linkNo = prevlink[i][j];

		displayPath((linkNo % n == 0) && linkNo != 0 ? linkNo / n - 1 : linkNo
				/ n, linkNo % n == 0 ? ((linkNo / n)) : ((linkNo % n) - 1),
				origin);
	}

	public static void display() {
		for (int i = 0; i < n; i++) {
			for (int j = 0; j < n; j++) {

				System.out.print(linkcost[i][j] + " ");
			}
			System.out.println();
		}
	}

	public static void displayLinkendCost() {

		for (int i = 0; i < n; i++) {
			for (int j = 0; j < n; j++) {

				System.out.print(linkendcost[i][j] + " ");
			}
			System.out.println();
		}

		for (int i = 0; i < n; i++) {
			for (int j = 0; j < n; j++) {

				System.out.print(prevlink[i][j] + " ");
			}
			System.out.println();
		}
	}

	public static double[][] getlinkcost() {
		return linkcost;
	}

	public static void setlinkcost(double adjmat[][]) {
		ShortestPath.linkcost = adjmat;
	}

	public static double[][] getLinkcost() {
		return linkcost;
	}

	public static void setLinkcost(double[][] linkcost) {
		ShortestPath.linkcost = linkcost;
	}

	public static int[][] getPrevlink() {
		return prevlink;
	}

	public static void setPrevlink(int[][] prevlink) {
		ShortestPath.prevlink = prevlink;
	}

	public static double[][] getLinkendcost() {
		return linkendcost;
	}

	public static void setLinkendcost(double[][] linkendcost) {
		ShortestPath.linkendcost = linkendcost;
	}

	public static double[][] getTurnpenalty() {
		return turnpenalty;
	}

	public static void setTurnpenalty(double[][] turnpenalty) {
		ShortestPath.turnpenalty = turnpenalty;
	}

	public static int getN() {
		return n;
	}

}
